#include "vehicle_lateral_speed_yaw_mpc.h"
namespace auto_ros
{
namespace control
{

VehicleLateralSpeedYawMpc::VehicleLateralSpeedYawMpc(const std::string dyn_mpc_kf_config_path, const std::string vehicle_lmpc_yaml_name, double rate)
	: VehicleLMpcBase(dyn_mpc_kf_config_path, vehicle_lmpc_yaml_name, rate)
{
	init_mpc();
	dynpar_map_ = {{"speed", 1.0}};
	ref_state_slover_ptr_->add_slack_states({3});
	ref_state_slover_ptr_->qp_.addConstraint(box(-10. / 180.0 * M_PI,
												 (ref_state_slover_ptr_->x_(0) + par(vehicle_dyn_ptr_->parm_["lf"]) * ref_state_slover_ptr_->x_(1)) / dynpar(dynpar_map_["speed"]) - ref_state_slover_ptr_->u_(0),
												 10. / 180.0 * M_PI));
	ref_state_slover_ptr_->qp_.addConstraint(box(-10. / 180.0 * M_PI,
												 (ref_state_slover_ptr_->x_(0) - par(vehicle_dyn_ptr_->parm_["lr"]) * ref_state_slover_ptr_->x_(1)) / dynpar(dynpar_map_["speed"]),
												 10. / 180.0 * M_PI));
	ref_state_slover_ptr_->init_solver_ptr();

} //
void VehicleLateralSpeedYawMpc::init_mpc()
{
	dis_dyn_ptr_ = vehicle_dyn_ptr_->discretization(1 / rate_, "tustin");
	vehicle_mpc_ptr_ = std::make_shared<Mpc>(dis_dyn_ptr_, mpc_yaml_["mpc_windows"].as<int>(),
											 1 /*max_xu_constraint*/, 0, 1 /*max_delta_u*/, 0, 0,
											 2 /*max_xu_slack_ineq = 0*/, 0, 0, 1 /*max_delta_u_ae*/, 0, 0, 0);
	vehicle_mpc_ptr_->set_xu_affine({}, {}, {0}, {1.0}, 0, -mpc_yaml_["max_tire_f_angle"].as<double>() / 180 * 3.14,
									mpc_yaml_["max_tire_f_angle"].as<double>() / 180 * 3.14);
	vehicle_mpc_ptr_->set_xu_slack_ineq({0, 1}, {1 / 1.0, vehicle_dyn_ptr_->dyn_yaml_["lf"].as<double>() / 1.0}, {0}, {-1}, 0,
										-mpc_yaml_["alpha_f_max"].as<double>() / 180 * 3.14,
										mpc_yaml_["alpha_f_max"].as<double>() / 180 * 3.14);
	vehicle_mpc_ptr_->set_xu_slack_ineq({0, 1}, {1 / 1.0, -vehicle_dyn_ptr_->dyn_yaml_["lr"].as<double>() / 1.0}, {}, {}, 1,
										-mpc_yaml_["alpha_r_max"].as<double>() / 180 * 3.14,
										mpc_yaml_["alpha_r_max"].as<double>() / 180 * 3.14);
	vehicle_mpc_ptr_->set_delta_u_ae(0, 0, 0);
	vehicle_mpc_ptr_->set_delta_u_ae_w({mpc_yaml_["delta_tire_steering_f_w"].as<double>() * rate_ * rate_});

	vehicle_mpc_ptr_->set_xu_slack_ineq_w({mpc_yaml_["tire_f_alpha_w"].as<double>(), mpc_yaml_["tire_r_alpha_w"].as<double>()});

	//int set_delta_u(unsigned int constraint_index, unsigned int u_index, double min, double max);
	vehicle_mpc_ptr_->set_delta_u(0, 0, -mpc_yaml_["max_steering_rate"].as<double>() / rate_,
								  mpc_yaml_["max_steering_rate"].as<double>() / rate_);
}
bool VehicleLateralSpeedYawMpc::step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
									 const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
									 double speed, const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, double tire_f_alpha_w,
									 double tire_r_alpha_w)
{
	vehicle_dyn_ptr_->para_update_matrix(speed);

	vehicle_dyn_ptr_->discrete(dis_dyn_ptr_, 1 / rate_, "tustin");
	/*bool Mpc::update_mpc(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
					 const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
					 const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w,
					 const std::vector<double> &xu_slack_ineq_w, const std::vector<double> &x_final_ae_w,
					 const std::vector<double> &x_final_slack_ineq_w, const std::vector<double> &delta_u_ae_w,
					 const std::vector<double> &delta_u_slack_ineq_w, const std::vector<double> &delta_x_ae_w,
					 const std::vector<double> &delta_x_slack_ineq_w)*/
	if (!vehicle_mpc_ptr_->update_mpc(x0, u_fb, x_ref, u_ref, distur_ref, q, r, {}, {tire_f_alpha_w, tire_r_alpha_w},
									  {}, {}, {}, {}, {}, {}))
	{
		std::cout << "\033[31m Error:VehicleLateralSpeedYawMpc::step failed,vehicle_mpc_ptr_->update_mpc. \033[0m" << std::endl;
		return false;
	}
	if (!(vehicle_mpc_ptr_->mpc_solve()))
	{
		std::cout << "\033[31m Error:VehicleLateralSpeedYawMpc::step failed,vehicle_mpc_ptr_->mpc_solve(). \033[0m" << std::endl;
		return false;
	}
	return true;
}
bool VehicleLateralSpeedYawMpc::step(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
									 double kappa, double speed)
{
	Eigen::MatrixXd q_temp = Eigen::MatrixXd::Zero(vehicle_dyn_ptr_->state_dim(), vehicle_dyn_ptr_->state_dim());
	q_temp(0, 0) = mpc_yaml_["lateral_speed_factor"].as<double>();
	q_temp(1, 1) = mpc_yaml_["yaw_rate_factor"].as<double>();
	q_temp(2, 2) = mpc_yaml_["lateral_error_factor"].as<double>();
	q_temp(3, 3) = mpc_yaml_["heading_error_factor"].as<double>();
	Eigen::MatrixXd r_temp = Eigen::MatrixXd::Zero(vehicle_dyn_ptr_->control_dim(), vehicle_dyn_ptr_->control_dim());
	if (std::abs(x0[2]) <= 0.01 && std::abs(x0[3]) < 0.01) //std::abs(distur_ref(0, 0)) < 0.001
	{
		r_temp(0, 0) = mpc_yaml_["steering_f_factor_max"].as<double>();
	}
	else
	{
		r_temp(0, 0) = mpc_yaml_["steering_f_factor_min"].as<double>();
	}

	Eigen::VectorXd x_ref(dis_dyn_ptr_->state_dim());
	Eigen::VectorXd u_ref(dis_dyn_ptr_->control_dim());
	Eigen::VectorXd e_y_ref(1);
	e_y_ref << 0.0;
	double speed_threshold = vehicle_dyn_ptr_->dyn_yaml_["speed_threshold"].as<double>();
	if (std::abs(speed) < speed_threshold)
	{
		if (speed >= 0)
			speed = speed_threshold;
		else
			speed = -speed_threshold;
	}
	vehicle_dyn_ptr_->para_update_matrix(speed);
	vehicle_dyn_ptr_->discrete(dis_dyn_ptr_, 1 / rate_, "tustin");
	/*calc_steady_state_ctr(
	const Eigen::VectorXd &distur_ref, const Eigen::VectorXd &y_controlled_ref,
	Eigen::VectorXd &steady_state, Eigen::VectorXd &steady_control)*/
	Eigen::VectorXd distur_ref(vehicle_dyn_ptr_->distur_dim());
	distur_ref << kappa;
	vehicle_dyn_ptr_->calc_steady_state_ctr(distur_ref, e_y_ref, x_ref, u_ref);
	//dis_dyn_ptr_->calc_steady_state_ctr(distur_ref, e_y_ref, x_ref, u_ref);
	/*Eigen::VectorXd slack_vec(vehicle_dyn_ptr_->state_dim());
	ref_state_slover_ptr_->calc_steady_state(distur_ref, x_ref, u_ref, slack_vec);*/
	x_ref_ = x_ref;
	u_ref_ = u_ref;
	vehicle_mpc_ptr_->set_xu_slack_ineq({0, 1}, {1 / speed, vehicle_dyn_ptr_->dyn_yaml_["lf"].as<double>() / speed}, {0}, {-1}, 0,
										-mpc_yaml_["alpha_f_max"].as<double>() / 180 * 3.14,
										mpc_yaml_["alpha_f_max"].as<double>() / 180 * 3.14);
	vehicle_mpc_ptr_->set_xu_slack_ineq({0, 1}, {1 / speed, -vehicle_dyn_ptr_->dyn_yaml_["lr"].as<double>() / speed}, {}, {}, 1,
										-mpc_yaml_["alpha_r_max"].as<double>() / 180 * 3.14,
										mpc_yaml_["alpha_r_max"].as<double>() / 180 * 3.14);

	double tire_f_alpha_w = mpc_yaml_["tire_f_alpha_w"].as<double>();
	double tire_r_alpha_w = mpc_yaml_["tire_r_alpha_w"].as<double>();

	/*bool Mpc::update_mpc(const Eigen::VectorXd &x0, const Eigen::VectorXd &u_fb,
					 const Eigen::MatrixXd &x_ref, const Eigen::MatrixXd &u_ref, const Eigen::MatrixXd &distur_ref,
					 const Eigen::MatrixXd &q, const Eigen::MatrixXd &r, const std::vector<double> &xu_ae_w,
					 const std::vector<double> &xu_slack_ineq_w, const std::vector<double> &x_final_ae_w,
					 const std::vector<double> &x_final_slack_ineq_w, const std::vector<double> &delta_u_ae_w,
					 const std::vector<double> &delta_u_slack_ineq_w, const std::vector<double> &delta_x_ae_w,
					 const std::vector<double> &delta_x_slack_ineq_w)*/

	if (!vehicle_mpc_ptr_->update_mpc(x0, u_fb, x_ref, u_ref, distur_ref, q_temp, r_temp, {}, {tire_f_alpha_w, tire_r_alpha_w},
									  {}, {}, {}, {}, {}, {}))
	{
		std::cout << "\033[31m Error:VehicleLateralSpeedYawMpc::step failed,vehicle_mpc_ptr_->update_mpc. \033[0m" << std::endl;
		return false;
	}
	if (!(vehicle_mpc_ptr_->mpc_solve()))
	{
		std::cout << "\033[31m Error:VehicleLateralSpeedYawMpc::step failed,vehicle_mpc_ptr_->mpc_solve(). \033[0m" << std::endl;
		return false;
	}
	return true;
}

bool VehicleLateralSpeedYawMpc::reset_kf()
{
	return true;
}
bool VehicleLateralSpeedYawMpc::reset_kf(const Eigen::VectorXd &state_esti)
{
	return true;
}
} // namespace control
} // namespace auto_ros
